#include "kfifo.h"
#include <string.h>

#define KFIFO_DOWN(size, align)      ((size) & ~((align) - 1))

void kfifo::init(uint8_t* data_ptr, uint16_t data_size)
{
    if (!data_ptr) {
        return;
    }
    if (!data_size) {
        return;
    }

    read_mirror = read_index = 0;
    write_mirror = write_index = 0;

    /* set buffer pool and size */
    buffer_ptr = data_ptr;
    buffer_size = KFIFO_DOWN(data_size, 4);
}

kfifo::kfifo(uint16_t data_size)
    :init_flag(1)
{
    buffer_ptr = new uint8_t[data_size];
    this->init(buffer_ptr, data_size);
}

kfifo::kfifo(uint8_t* data_ptr, uint16_t data_size)
    :init_flag(0)
{
    this->init(data_ptr, data_size);
}

kfifo::~kfifo()
{
    if (this->init_flag == 1) {
        if (buffer_ptr != NULL) {
            delete buffer_ptr;
        }
    }
}

size_t kfifo::kfifo_put(const uint8_t* ptr, size_t length)
{
    size_t size;

    /* whether has enough space */
    size = kfifo_data_space_len();

    /* no space */
    if (size == 0)
        return 0;

    /* drop some data */
    if (size < length)
        length = size;

    if (buffer_size - write_index > length)
    {
        /* read_index - write_index = empty space */
        memcpy(&buffer_ptr[write_index], ptr, length);
        /* this should not cause overflow because there is enough space for
         * length of data in current mirror */
        write_index += length;
        return length;
    }

    memcpy(&buffer_ptr[write_index],
        &ptr[0],
        buffer_size - write_index);
    memcpy(&buffer_ptr[0],
        &ptr[buffer_size - write_index],
        length - (buffer_size - write_index));

    /* we are going into the other side of the mirror */
    write_mirror = ~write_mirror;
    write_index = length - (buffer_size - write_index);

    return length;
}

size_t kfifo::kfifo_put_force(const uint8_t* ptr, size_t length)
{
    size_t space_length;

    space_length = kfifo_data_space_len();

    if (length > buffer_size)
    {
        ptr = &ptr[length - buffer_size];
        length = buffer_size;
    }

    if (buffer_size - write_index > length)
    {
        /* read_index - write_index = empty space */
        memcpy(&buffer_ptr[write_index], ptr, length);
        /* this should not cause overflow because there is enough space for
         * length of data in current mirror */
        write_index += length;

        if (length > space_length)
            read_index = write_index;

        return length;
    }

    memcpy(&buffer_ptr[write_index],
        &ptr[0],
        buffer_size - write_index);
    memcpy(&buffer_ptr[0],
        &ptr[buffer_size - write_index],
        length - (buffer_size - write_index));

    /* we are going into the other side of the mirror */
    write_mirror = ~write_mirror;
    write_index = length - (buffer_size - write_index);

    if (length > space_length)
    {
        if (write_index <= read_index)
            read_mirror = ~read_mirror;
        read_index = write_index;
    }

    return length;
}

size_t kfifo::kfifo_get(uint8_t* ptr, size_t length)
{
    size_t size;

    /* whether has enough data  */
    size = kfifo_data_len();

    /* no data */
    if (size == 0)
        return 0;

    /* less data */
    if (size < length)
        length = size;

    if (buffer_size - read_index > length)
    {
        /* copy all of data */
        memcpy(ptr, &buffer_ptr[read_index], length);
        /* this should not cause overflow because there is enough space for
         * length of data in current mirror */
        read_index += length;
        return length;
    }

    memcpy(&ptr[0],
        &buffer_ptr[read_index],
        buffer_size - read_index);
    memcpy(&ptr[buffer_size - read_index],
        &buffer_ptr[0],
        length - (buffer_size - read_index));

    /* we are going into the other side of the mirror */
    read_mirror = ~read_mirror;
    read_index = length - (buffer_size - read_index);

    return length;
}

size_t kfifo::kfifo_peek(uint8_t** ptr)
{
    size_t size;

    *ptr = NULL;

    /* whether has enough data  */
    size = kfifo_data_len();

    /* no data */
    if (size == 0)
        return 0;

    *ptr = &buffer_ptr[read_index];

    if ((size_t)(buffer_size - read_index) > size)
    {
        read_index += (uint16_t)size;
        return size;
    }

    size = buffer_size - read_index;

    /* we are going into the other side of the mirror */
    read_mirror = ~read_mirror;
    read_index = 0;

    return size;
}

size_t kfifo::kfifo_putchar(const uint8_t ch)
{
    /* whether has enough space */
    if (!kfifo_data_space_len())
        return 0;

    buffer_ptr[write_index] = ch;

    /* flip mirror */
    if (write_index == buffer_size - 1)
    {
        write_mirror = ~write_mirror;
        write_index = 0;
    }
    else
    {
        write_index++;
    }

    return 1;
}

size_t kfifo::kfifo_putchar_force(const uint8_t ch)
{
    enum kfifo_state old_state;

    old_state = kfifo_status();

    buffer_ptr[write_index] = ch;

    /* flip mirror */
    if (write_index == buffer_size - 1)
    {
        write_mirror = ~write_mirror;
        write_index = 0;
        if (old_state == kfifo_state::KFIFO_FULL)
        {
            read_mirror = ~read_mirror;
            read_index = write_index;
        }
    }
    else
    {
        write_index++;
        if (old_state == kfifo_state::KFIFO_FULL)
            read_index = write_index;
    }

    return 1;
}

size_t kfifo::kfifo_getchar(uint8_t* ch)
{
    /* ringbuffer is empty */
    if (!kfifo_data_len())
        return 0;

    /* put byte */
    *ch = buffer_ptr[read_index];

    if (read_index == buffer_size - 1)
    {
        read_mirror = ~read_mirror;
        read_index = 0;
    }
    else
    {
        read_index++;
    }

    return 1;
}

size_t kfifo::kfifo_getchar_force(uint8_t* ch)
{
    switch (kfifo_status())
    {
    case kfifo_state::KFIFO_EMPTY:
        return 0;
    case kfifo_state::KFIFO_FULL:
        return buffer_size;
    case kfifo_state::KFIFO_HALFFULL:
    default:
    {
        size_t wi = write_index, ri = read_index;

        if (wi > ri)
            return wi - ri;
        else
            return buffer_size - (ri - wi);
    }
    }
}

size_t kfifo::kfifo_data_len(void)
{
    switch (kfifo_status())
    {
    case kfifo_state::KFIFO_EMPTY:
        return 0;
    case kfifo_state::KFIFO_FULL:
        return this->buffer_size;
    case kfifo_state::KFIFO_HALFFULL:
    default:
    {
        size_t wi = this->write_index, ri = this->read_index;

        if (wi > ri)
            return wi - ri;
        else
            return this->buffer_size - (ri - wi);
    }
    }
}

size_t kfifo::kfifo_data_space_len(void)
{
    return buffer_size - kfifo_data_len();
}

void kfifo::kfifo_reset(void)
{
    read_mirror = 0;
    read_index = 0;
    write_mirror = 0;
    write_index = 0;
}
